﻿#include <algorithm>
#include <gtest/gtest.h>
#include <iostream>
#include <vector>

#include <learn/alg/alg_interpolator.h>
#include <learn/utils/output.h>

using namespace xi::alg;
using namespace xi::geom;

void test_bezier(const std::vector<double> &u, std::function<Vec2(double)> f)
{
    Alg_Bezier_Interpolator<2> bezier;
    bezier.x = u;
    for (auto &t : u)
        bezier.f.push_back(f(t));
    auto curve = bezier.interpolate();

    auto checkError = [](const auto &curve, const auto &f, double first, double last) {
        double error = 0.0;
        int N = 1000;
        for (int i = 0; i < N; i++)
        {
            double t = first + (last - first) * i / (N - 1);
            Vec2 p = curve->value(t);
            Vec2 fp = f(t);
            error += (p - fp).square_modulus();
        }
        return error / N;
    };

    std::cout << "Bezier error: " << checkError(curve, f, 0, 1) << std::endl;
}

TEST(TestSample, A)
{
    auto f = [](double x) { return Vec2{x * sin(2 * Numeric::pi * x) * cos(2 * Numeric::pi * x), 0.0}; };

    int sample = 10;
    double L = 0;
    std::vector<double> u(sample);
    std::vector<double> s(sample);
    std::vector<double> c(sample, 0);
    for (int i = 0; i < sample; i++)
    {
        u[i] = i / (sample - 1.0);
        if (i > 0)
            L += (f(u[i]) - f(u[i - 1])).modulus();
        s[i] = L;
    }
    double C = 0;
    for (int i = 1; i < sample - 1; i++)
    {
        C += (((f(u[i + 1]) - f(u[i])) / (u[i + 1] - u[i]) - (f(u[i]) - f(u[i - 1])) / (u[i] - u[i - 1])) /
              (u[i + 1] - u[i - 1]))
                 .modulus();
        c[i] = C;
    }
    std::cout << "c: " << c << std::endl;
    std::cout << "u: " << u << std::endl;

    test_bezier(u, f);

    int times = 30;
    while (times--)
    {
        C = 0, L = 0;
        std::vector<double> nu(sample), ns(sample);
        nu[0] = u[0], nu[sample - 1] = u[sample - 1];
        ns[0] = s[0], ns[sample - 1] = s[sample - 1];
        for (int i = 1; i < sample - 1; i++)
        {
            double r = (s[i] - s[i - 1]) / (s[i + 1] - s[i - 1]);
            // nu[i] = (u[i] + u[i - 1]) / 2 * r + (u[i] + u[i + 1]) / 2 * (1 - r);
            nu[i] = u[i - 1] * r + u[i + 1] * (1 - r);

            L += (f(u[i]) - f(u[i - 1])).modulus();
            ns[i] = L;
        }

        u = std::move(nu);
        s = std::move(ns);
        // std::cout << u << std::endl;
        // std::cout << s << std::endl;

        test_bezier(u, f);
    }
}